
/*
 * File:   PSO.cpp
 * Author: werty
 *
 * Created on 27 kwiecień 2011, 23:10
 */

#include "MOPSO.h"
#include <stdlib.h>
#include <QDebug>
#include <qt4/QtCore/qglobal.h>

MOPSO::MOPSO()
{
    // parser.DefineVar("x", &this->xVal);
    // parser.DefineVar("y", &this->yVal);
}

MOPSO::MOPSO(int wiel_pop, Problem * problem)
{
    qsrand(time.toTime_t());

    // parser.DefineVar("x", &this->xVal);
    // parser.DefineVar("y", &this->yVal);
    this -> il_hiperkostek = 30;
    this -> problem        = problem;
    this -> il_funkcji     = problem -> funkcje.size();
    this -> il_ograniczen  = problem -> ograniczenia.size();
    this -> min_fitness    = new float[problem -> funkcje.size()];
    this -> max_fitness    = new float[problem -> funkcje.size()];
    this -> dx_fitness     = new float[problem -> funkcje.size()];
    this -> il_zmiennych   = problem -> zmienne.size();

    for (int i = 0; i < wiel_pop; i++)
    {
        populacja.push_back(new Particle(il_zmiennych, il_funkcji, il_ograniczen));
    }
}

MOPSO::~MOPSO()
{
}

void MOPSO::aktualizuj_wartosci()
{
    for (int i = 0; i < populacja.size(); i++)
    {
        for (int x_ind = 0; x_ind < this -> il_zmiennych; x_ind++)
        {
            problem -> zmienne[x_ind] -> zmienna = populacja[i] -> x[x_ind];
        }

        for (int f_ind = 0; f_ind < this -> il_funkcji; f_ind++)
        {
            populacja[i] -> fitness[f_ind] = problem -> funkcje[f_ind] -> Eval();
        }
    }
}

/*
 *
 *
 *
 */
void MOPSO::Inicjuj()
{
    max_iter=30;
    num_iter=0;
    x_width = 50;
    xmin    = -100;
    xmax    = 100;

    qDebug() << "inicjuje...\n";

    for (int i = 0; i < populacja.size(); i++)
    {
        for (int x_ind = 0; x_ind < this -> il_zmiennych; x_ind++)
        {
            x_width                  = problem -> zmienne[x_ind] -> max - problem -> zmienne[x_ind] -> min;
            xmin                     = problem -> zmienne[x_ind] -> min;
            populacja[i] -> v[x_ind] = 0.0;    // (float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax;
            populacja[i] -> x[x_ind] = (float) rand() / (float) RAND_MAX * x_width + xmin;

            // qDebug()<<"x "<<populacja[i]->x[x_ind] <<"\n";
            problem -> zmienne[x_ind] -> zmienna = populacja[i] -> x[x_ind];
        }

        for (int f_ind = 0; f_ind < this -> il_funkcji; f_ind++)
        {
            populacja[i] -> fitness[f_ind]       = problem -> funkcje[f_ind] -> Eval();
            populacja[i] -> fitness_pbest[f_ind] = populacja[i] -> fitness[f_ind];
        }

        pbests.push_back(new Particle(*populacja[i]));

        // populacja[i]->v[1] = (float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax;
        // populacja[i]->x[1] = (float)rand() / (float)RAND_MAX * x_width + xmin;
        // qDebug()<<i<<" x1 "<<particles_vec[i]->x_pos[0]<<" x2 "<<particles_vec[i]->x_pos[1]<<"\n";
        // populacja[i]->pbest_pos[0] = populacja[i]->x[0];
        // populacja[i]->pbest_pos[1] = populacja[i]->x[1];
        // xVal = populacja[i]->x[0];
        // yVal = populacja[i]->x[1];
        // populacja[i]->fitness = (double)parser.Eval();
        // populacja[i]->fitness_pbest = populacja[i]->fitness;
    }

    // wyznaczanie niezdominowanych rozwiazan
    sprawdz_zdominowanie();

    for (int i_pop = 0; i_pop < populacja.size(); i_pop++)
    {
        if (!populacja[i_pop] -> zdominowana)
        {
            repozytorium.push_back(new Particle(*populacja[i_pop]));
        }
        else
        {
        }
    }

    for (int i = 0; i < populacja.size(); i++)
    {
        qDebug() << "rozw " << i << " (";

        for (int j = 0; j < il_funkcji; j++)
        {
            qDebug() << populacja[i] -> fitness[j] << " ";
        }

        qDebug() << ")\n";
    }

    qDebug() << "\n\n";

    for (int i = 0; i < repozytorium.size(); i++)
    {
        qDebug() << "rozw niezdominowane " << i << " (";

        for (int j = 0; j < il_funkcji; j++)
        {
            qDebug() << repozytorium[i] -> fitness[j] << " ";
        }

        qDebug() << ")\n";
    }

    qDebug() << "zainicjowalem\n";
}

void MOPSO::Iteruj()
{
    // for (int i = 0; i < populacja.size(); i++) {
    //
    // xVal = populacja[i]->x[0];
    // yVal = populacja[i]->x[1];
    // populacja[i]->fitness = parser.Eval();
    //
    // if (maximize) {
    // if (populacja[i]->fitness > populacja[i]->fitness_pbest) {
    // populacja[i]->pbest_pos[0] = populacja[i]->x[0];
    // populacja[i]->pbest_pos[1] = populacja[i]->x[1];
    //
    // populacja[i]->fitness_pbest = populacja[i]->fitness;
    // }
    // if (populacja[i]->fitness > fitness_gbest) {
    // gbest_pos[0] = populacja[i]->x[0];
    // gbest_pos[1] = populacja[i]->x[1];
    //
    // fitness_gbest = populacja[i]->fitness;
    // }
    //
    // } else {
    // if (populacja[i]->fitness < populacja[i]->fitness_pbest) {
    // populacja[i]->pbest_pos[0] = populacja[i]->x[0];
    // populacja[i]->pbest_pos[1] = populacja[i]->x[1];
    //
    // populacja[i]->fitness_pbest = populacja[i]->fitness;
    // }
    // if (populacja[i]->fitness < fitness_gbest) {
    // gbest_pos[0] = populacja[i]->x[0];
    // gbest_pos[1] = populacja[i]->x[1];
    //
    // fitness_gbest = populacja[i]->fitness;
    // }
    //
    // }
    //
    //
    //
    //
    //
    //
    //
    // }
    for (int i = 0; i < populacja.size(); i++)
    {
        // particles_vec[i]->v[0]=czi*(particles_vec[i]->v[0]+
        // c_p*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(particles_vec[i]->pbest_pos[0]-particles_vec[i]->x_pos[0])+
        // c_g*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(gbest_pos[0]-particles_vec[i]->x_pos[0]));
        //
        // particles_vec[i]->v[1]=czi*(particles_vec[i]->v[1]+
        // c_p*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(particles_vec[i]->pbest_pos[1]-particles_vec[i]->x_pos[1])+
        // c_g*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(gbest_pos[1]-particles_vec[i]->x_pos[1]));
        //
        populacja[i] -> v[0] = /* czi* */ (populacja[i] -> v[0]
                + c_p * ((float) rand() / (float) RAND_MAX) * (populacja[i] -> pbest_pos[0] - populacja[i] -> x[0])
                + c_g * ((float) rand() / (float) RAND_MAX) * (gbest_pos[0] - populacja[i] -> x[0]));
        populacja[i] -> v[1] = /* czi* */ (populacja[i] -> v[1]
                + c_p * ((float) rand() / (float) RAND_MAX) * (populacja[i] -> pbest_pos[1] - populacja[i] -> x[1])
                + c_g * ((float) rand() / (float) RAND_MAX) * (gbest_pos[1] - populacja[i] -> x[1]));

        if (populacja[i] -> v[0] > vmax)
        {
            populacja[i] -> v[0] = vmax;
        }

        if (populacja[i] -> v[1] > vmax)
        {
            populacja[i] -> v[1] = vmax;
        }

        populacja[i] -> x[0] = populacja[i] -> x[0] + w * populacja[i] -> v[0];
        populacja[i] -> x[1] = populacja[i] -> x[1] + w * populacja[i] -> v[1];

        if (populacja[i] -> x[0] < xmin)
        {
            populacja[i] -> x[0] = xmin;
        }

        if (populacja[i] -> x[1] < xmin)
        {
            populacja[i] -> x[1] = xmin;
        }

        if (populacja[i] -> x[0] > xmax)
        {
            populacja[i] -> x[0] = xmax;
        }

        if (populacja[i] -> x[1] > xmax)
        {
            populacja[i] -> x[1] = xmax;
        }
    }
}

void MOPSO::generuj_kostki()
{
    hiperkostki.clear();

    // wyznaczamy min i max wartosci funkcji kryterialnych w naszej populacji
    for (int i = 0; i < il_funkcji; i++)
    {
        max_fitness[i] = populacja[0] -> fitness[i];
        min_fitness[i] = populacja[0] -> fitness[i];
    }

    for (int i = 0; i < populacja.size(); i++)
    {
        for (int j = 0; j < il_funkcji; j++)
        {
            if (populacja[i] -> fitness[j] > max_fitness[j])
            {
                max_fitness[j] = populacja[i] -> fitness[j];
            }

            if (populacja[i] -> fitness[j] < min_fitness[j])
            {
                min_fitness[j] = populacja[i] -> fitness[j];
            }
        }
    }

    // wyznaczamy szerokosc jednej hiperkostki
    for (int i = 0; i < il_funkcji; i++)
    {
        qDebug() << "max  " << i << " =" << max_fitness[i];
        qDebug() << "min  " << i << " =" << min_fitness[i];

        dx_fitness[i] = (max_fitness[i] - min_fitness[i]) / il_hiperkostek;

        qDebug() << "dx  " << i << " =" << dx_fitness[i] << "\n";
    }

    int tmp;

    for (int i = 0; i < repozytorium.size(); i++)
    {
        hiperkostki.push_back(Hiperkostka());
        hiperkostki[i].czasteczki.push_back(i);

        // qDebug() << "rep " << i << " (";
        for (int j = 0; j < il_funkcji; j++)
        {
            tmp = (int) ((repozytorium[i] -> fitness[j] - min_fitness[j]) / dx_fitness[j]);

            // qDebug() << tmp << " ";
            hiperkostki[i].polozenie.push_back(tmp);
        }

        // qDebug() << ")\n";
    }

    qSort(hiperkostki.begin(), hiperkostki.end(), lessThenHiperkostka);

    // qDebug() << "il hip " << hiperkostki.size();
    // qDebug() << "po sortowaniu\n";
    for (int i = 0; i < hiperkostki.size(); i++)
    {
        // qDebug() << "h " << i << "(";
        for (int j = 0; j < il_funkcji; j++)
        {
            // qDebug() << hiperkostki[i].polozenie[j];
        }

        // qDebug() << " ind " << hiperkostki[i].czasteczki[0];
        // qDebug() << ")\n";
    }

    // Hiperkostka h1, h2, h3, h4;
    // h1.polozenie << 1 << 3 << 66;
    // h2.polozenie << 1 << 3 << 4;
    // h3.polozenie << 2 << 7 << 4;
    // h4.polozenie << 1 << 5 << 4;
    // QVector<Hiperkostka> hip;
    // hip << h1 << h2 << h3 << h4;
    // qSort(hip.begin(), hip.end(), lessThenHiperkostka);
    //
    // for (int i = 0; i < hip.size(); i++) {
    // qDebug() << "ht " << i << "(";
    // for (int j = 0; j < 3; j++) {
    // qDebug() << hip[i].polozenie[j];
    //
    // }
    //
    // qDebug() << ")\n";
    //
    // }
    //
    // qDebug() << "compare  2 7 4 to  1 3 66 = " << lessThenHiperkostka(h3, h1) << "\n";
    //
    // QVector<Hiperkostka>::iterator it = qBinaryFind(hip.begin(), hip.end(), hip[1], lessThenHiperkostka);
    // qDebug() << "it " << it->polozenie[0] << " " << it->polozenie[1] << "\n";
    // qSort(hip.begin(), hip.end(), qGreater< QVector<int> >());
    // QVector<int> v1,v2;
    // v1<<1<<3<<3;
    // v2<<1<<3<<4;
    joinHiperkostki(hiperkostki);

    for (int i = 0; i < hiperkostki.size(); i++)
    {
        // qDebug() << "ht " << i << "(pol ";
        for (int j = 0; j < hiperkostki[i].polozenie.size(); j++)
        {
            // qDebug() << hiperkostki[i].polozenie[j] << " ";
        }

        // qDebug() << " ind: ";
        for (int j = 0; j < hiperkostki[i].czasteczki.size(); j++)
        {
            // qDebug() << hiperkostki[i].czasteczki[j] << " ";
        }

        // qDebug() << ")\n";
    }

    // qDebug() << "v[1]=" << hip[0].polozenie[1] << hip[1].polozenie[1] << hip[2].polozenie[1];
}

int MOPSO::losuj_kostke_ruletka()
{
    float * tab  = new float[hiperkostki.size()];
    float   maks = 0;

    for (int i = 0; i < hiperkostki.size(); i++)
    {
        // qDebug()<<"size "<<hiperkostki[i].ind_czastek.size()<<"\n";
        tab[i] = maks + 10.0 / (float) hiperkostki[i].czasteczki.size();

        // qDebug()<<"tab i "<<tab[i]<<"\n";
        maks = tab[i];
    }

    // qDebug()<<"maks "<<maks<<"\n";
    // qDebug()<<"time_t "<<time.toTime_t()<<"\n";
    float random_float = maks * qrand() / RAND_MAX;

    // qDebug()<<"random float "<<random_float<<"\n";
    for (int i = 0; i < hiperkostki.size(); i++)
    {
        if (random_float <= tab[i])
        {
            return i;
        }
    }
}

void MOPSO::aktualizuj_predkosci()
{
    int h;
    int h_rep;

    for (int i = 0; i < populacja.size(); i++)
    {
        // particles_vec[i]->v[0]=czi*(particles_vec[i]->v[0]+
        // c_p*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(particles_vec[i]->pbest_pos[0]-particles_vec[i]->x_pos[0])+
        // c_g*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(gbest_pos[0]-particles_vec[i]->x_pos[0]));
        //
        // particles_vec[i]->v[1]=czi*(particles_vec[i]->v[1]+
        // c_p*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(particles_vec[i]->pbest_pos[1]-particles_vec[i]->x_pos[1])+
        // c_g*((float)rand() / (float)RAND_MAX * vmax - 0.5 * vmax)*0.1*(gbest_pos[1]-particles_vec[i]->x_pos[1]));
        //
        h     = losuj_kostke_ruletka();
        h_rep = (int) ((float) hiperkostki[h].czasteczki.size() * qrand() / RAND_MAX);

        // qDebug() << "h_rep " << h_rep << " z " << hiperkostki[h].czasteczki.size() << "\n";
        w = 1;

        for (int i_v = 0; i_v < problem -> zmienne.size(); i_v++)
        {
            populacja[i] ->
                 v[i_v] = (w * populacja[i] -> v[i_v]
                           + ((float) qrand() / (float) RAND_MAX) * (pbests[i] -> x[i_v] - populacja[i] -> x[i_v])
                           + ((float) qrand() / (float) RAND_MAX)
                             * (repozytorium[hiperkostki[h].czasteczki[h_rep]] -> x[i_v] - populacja[i] -> x[i_v]));

            qDebug() << "populacja[i]->v[i_v]" << populacja[i] -> v[i_v] << "\n";
            qDebug() << "populacja[i]->x[i_v]" << populacja[i] -> x[i_v] << "\n";
            qDebug() << "repozytorium[hiperkostki[h].czasteczki[h_rep]]-> x[i_v] = "
                     << repozytorium[hiperkostki[h].czasteczki[h_rep]] -> x[i_v] << "\n";
            qDebug() << "pbests[i]->x[i_v] " << pbests[i] -> x[i_v] << "\n";
            qDebug() << "w " << w << "\n";

            if (populacja[i] -> v[i_v] > vmax)
            {
                populacja[i] -> v[i_v] = vmax;
            }

            if (populacja[i] -> v[i_v] < -vmax)
            {
                populacja[i] -> v[i_v] = -vmax;
            }

            qDebug() << "populacja[i]->v[i_v] " << populacja[i] -> v[i_v] << "\n";
        }
    }
}

void MOPSO::aktualizuj_pozycje()
{
    for (int i = 0; i < populacja.size(); i++)
    {
        for (int i_v = 0; i_v < problem -> zmienne.size(); i_v++)
        {
            populacja[i] -> x[i_v] = populacja[i] -> x[i_v] + populacja[i] -> v[i_v];

            qDebug() << "x_old " << populacja[i] -> x[i_v] << " v " << populacja[i] -> v[i_v] << "\n";

            if (populacja[i] -> x[i_v] < problem -> zmienne[i_v] -> min)
            {
                populacja[i] -> x[i_v] = problem -> zmienne[i_v] -> min;
                populacja[i] -> v[i_v] = -populacja[i] -> v[i_v];
            }

            if (populacja[i] -> x[i_v] > problem -> zmienne[i_v] -> max)
            {
                populacja[i] -> x[i_v] = problem -> zmienne[i_v] -> max;
                populacja[i] -> v[i_v] = -populacja[i] -> v[i_v];
            }
        }
    }
}

void MOPSO::aktualizuj_pbest()
{
    for (int i = 0; i < populacja.size(); i++)
    {
        if (populacja[i] > pbests[i])           // dominuje nowe przystosowanie
        {
            (*pbests[i]) = (*populacja[i]);
        }
        else
        {
            if (!(pbests[i] > populacja[i]))    // zaden nie dominuje wiec losujemy
            {
                if ((qrand() / RAND_MAX) > 0.5)
                {
                    (*pbests[i]) = (*populacja[i]);
                }
            }
        }
    }
}

void MOPSO::sprawdz_zdominowanie()
{
    // wyznaczanie niezdominowanych rozwiazan
    bool zdominowany = false;

    for (int i_pop = 0; i_pop < populacja.size(); i_pop++)
    {
        zdominowany = false;

        for (int i_pop2 = 0; i_pop2 < populacja.size(); i_pop2++)
        {
            // qDebug() << "prownuje ------";
            if (problem -> Dominuje((*populacja[i_pop2]), (*populacja[i_pop])))
            {
                zdominowany = true;

                // qDebug() << "zdominowany";
                populacja[i_pop] -> zdominowana = true;

                // qDebug() << "117";
                break;
            }

            // qDebug() << "niezdominowany";
            populacja[i_pop] -> zdominowana = false;

            // qDebug() << "127";
        }
    }
}

void MOPSO::aktualizuj_repozytorium()
{
    bool zdominowany;

    for (int i = 0; i < populacja.size(); i++)
    {
        if (!populacja[i] -> zdominowana)    // niezdominowany wsrod osobnikow aktualnej pop
        {
            // sprawdzamy czy nie jest zdominowany przez roz. z rep.
            zdominowany = false;

            for (int j = 0; j < repozytorium.size(); j++)
            {
                if (!zdominowany && problem -> Dominuje((*repozytorium[j]), (*populacja[i])))
                {
                    // zdominowany wiec przechodzimy do nast. osob. z pop.
                    // continue;
                    zdominowany = true;
                }

                // nowy dominuje czastke z repozytorum wiec usuwamy ja z repozytorium
                if (problem -> Dominuje((*populacja[i]), (*repozytorium[j])))
                {
                    repozytorium.remove(j);

                    j--;
                }
            }

            if (!zdominowany)
            {
                // niezdominowany wiec dodajemy do repozytorium
                repozytorium.push_back(new Particle(*populacja[i]));
            }
        }
    }
}

void MOPSO::wyswietl_repozytorium()
{
    for (int i = 0; i < repozytorium.size(); i++)
    {
        qDebug() << "rep[" << i << "]:";

        for (int j = 0; j < il_zmiennych; j++)
        {
            qDebug() << "x:" << repozytorium[i] -> x[j] << " ";
        }

        for (int j = 0; j < il_zmiennych; j++)
        {
            qDebug() << "v:" << repozytorium[i] -> v[j] << " ";
        }

        qDebug() << "\n";
    }
}

void MOPSO::wyswietl_populacje()
{
    for (int i = 0; i < populacja.size(); i++)
    {
        qDebug() << "pop[" << i << "]:";

        for (int j = 0; j < il_zmiennych; j++)
        {
            qDebug() << "x:" << populacja[i] -> x[j] << " ";
        }

        for (int j = 0; j < il_zmiennych; j++)
        {
            qDebug() << "v:" << populacja[i] -> v[j] << " ";
        }

        qDebug() << "\n";
    }
}

void MOPSO::sprawdz_ograniczenia()
{
    double val_left, val_right;

    for (int ind_pop = 0; ind_pop < populacja.size(); ind_pop++)
    {
        for (int ind_ogr = 0; ind_ogr < problem -> ograniczenia.size(); ind_ogr++)
        {
            populacja[ind_pop] -> il_przek_ograniczen = 0;

            for (int x_ind = 0; x_ind < this -> il_zmiennych; x_ind++)
            {
                problem -> zmienne[x_ind] -> zmienna = populacja[ind_pop] -> x[x_ind];
            }

            val_left  = problem -> ograniczenia[ind_ogr] -> lewa_funkcja -> Eval();
            val_right = problem -> ograniczenia[ind_ogr] -> prawa_funkcja -> Eval();

            switch (problem -> ograniczenia[ind_ogr] -> rodzaj)
            {              // sprawdzamy czy ograniczenie nie zostalo przekroczone
                case 0:    // <
                    if (val_left >= val_right)
                    {
                        populacja[ind_pop] -> il_przek_ograniczen++;
                    }

                    break;

                case 1:    // <=
                    if (val_left > val_right)
                    {
                        populacja[ind_pop] -> il_przek_ograniczen++;
                    }

                    break;

                case 2:    // =
                    if (val_left != val_right)
                    {
                        populacja[ind_pop] -> il_przek_ograniczen++;
                    }

                    break;

                case 3:    // >=
                    if (val_left < val_right)
                    {
                        populacja[ind_pop] -> il_przek_ograniczen++;
                    }

                    break;

                case 4:    // >
                    if (val_left <= val_right)
                    {
                        populacja[ind_pop] -> il_przek_ograniczen++;
                    }

                    break;
            }
        }
    }
}


void MOPSO::mutuj(int gen, int il_gen, double wsp_mutacji)
{
    double wsp=pow((1.0-(double)gen/(double)il_gen),5.0/wsp_mutacji);
    if(wsp<=0)
    {
        return;
    
    }
    
    int wyb_wym;
    double zakres_mutacji;
    double min,max;
    
    for (int i_pop = 0; i_pop < populacja.size(); i_pop++) {
        
        
        wyb_wym=losuj(0,problem->zmienne.size()-1);
        zakres_mutacji=(problem->zmienne[wyb_wym]->max-problem->zmienne[wyb_wym]->min)*wsp;
        max=populacja[i_pop]->x[wyb_wym]+zakres_mutacji;
        min=populacja[i_pop]->x[wyb_wym]-zakres_mutacji;
        
        if(min<problem->zmienne[wyb_wym]->min)
        {
            min=problem->zmienne[wyb_wym]->min;
            
        }

        if(max>problem->zmienne[wyb_wym]->max)
        {
            max=problem->zmienne[wyb_wym]->max;
            
        }

        
        populacja[i_pop]->x[wyb_wym]=losuj(min,max);
    }

    
    
    
    
}
